#include "FaceDetector.h"
#include "FaceDetectorDLL.h"

#include <cmath>
#include <cassert>
using namespace std;

const double EPSILON = 0.001;

CFaceDetector::CFaceDetector(void)
{
	m_faceDetector = new CFaceDetectorDLL;
	m_eyeDetector = new CComponentDetectorDLL;
	m_faceDetector->Init();
	m_eyeDetector->Init();
}

CFaceDetector::~CFaceDetector(void)
{
	delete m_eyeDetector;
	delete m_faceDetector;
}

unsigned char* CFaceDetector::CreateBuffer(IplImage* img )
{
	if( NULL == img )
		return NULL;

	unsigned char* buffer = NULL;

	buffer = new unsigned char[img->imageSize];

	if( 0 == img->origin )
	{
		memcpy_s( buffer, img->imageSize * sizeof(unsigned char), 
			img->imageData, img->imageSize * sizeof(unsigned char) );
	}
	else
	{
		for( int h = 0; h < img->height; ++h )
		{
			int bufferStride = h * img->widthStep;
			int imgStride = ( img->height - 1 - h ) * img->widthStep;

			memcpy_s( buffer + bufferStride, img->widthStep * sizeof(unsigned char),
				img->imageData + imgStride, img->widthStep * sizeof(unsigned char) );
		}
	}

	return buffer;
}

void CFaceDetector::ReleaseBuffer(unsigned char* & buffer )
{
	if( buffer )
	{
		delete[] buffer;
		buffer = NULL;
	}
}

HRESULT CFaceDetector::Process(IplImage* img, vector<FaceComp> & result )
{
	if( NULL == img )
		return E_FAIL;

	FaceRect* rects = NULL;
	int faceNum = 0;
	int pixelBytes = 3;

	// Set parameters & options
	m_faceDetector->SetParam( FALSE, E_INPLANE_LEVEL2, TRUE, 600, 0, 50 );
	m_faceDetector->SetOptions( FALSE, FALSE, TRUE, FALSE, TRUE, TRUE, TRUE );

	// Detect faces
	unsigned char* buffer = NULL;
	buffer = this->CreateBuffer( img );
	m_faceDetector->DetectFace( img->width, img->height, buffer, img->widthStep,
		pixelBytes, 1, TRUE, NULL, &faceNum, &rects );	

	// Check results
	if( faceNum > 0 && rects )
	{
		// Copy rectangles
		for( int i = 0; i < faceNum; ++i )
		{
			result.push_back( MakeFaceComp( rects[i].rBox ) );
		}
		// Release face rectangles
		m_faceDetector->FreeFaceRect( rects );
		rects = NULL;

		// Detect eyes
		ComponentPosition leftEye;
		ComponentPosition rightEye;
		for( int i = 0; i < faceNum; ++i )
		{
			HRESULT hr = m_eyeDetector->DetectEye( img->width, img->height,
				buffer, img->widthStep, pixelBytes, 1, TRUE,
				&result[i].FaceBound, &leftEye, &rightEye );

			if( !SUCCEEDED(hr) )
			{
				result[i].IsEyeDetected = false;
			}
			else
			{
				result[i].IsEyeDetected = true;
				result[i].LeftEye = leftEye.ptPosition;
				result[i].RightEye = rightEye.ptPosition;
			}
		}		
	}

	this->ReleaseBuffer( buffer );

	return S_OK;
}

vector<IplImage*> CFaceDetector::Crop( IplImage* img, const vector<FaceComp> & comps )
{
	vector<IplImage*> result;
	for( int i = 0; i < (int)comps.size(); ++i )
	{
		int lx = comps[i].LeftEye.x;
		int ly = comps[i].LeftEye.y;
		int rx = comps[i].RightEye.x;
		int ry = comps[i].RightEye.y;

		double factor = DIST(lx, ly, rx, ry) / DIST(LX, LY, RX, RY);
		double angle = ANGLE(LX, LY, RX, RY) - ANGLE(lx, ly, rx, ry);

		float m[6];
		m[0] = (float) (factor * cos(angle));
		m[1] = (float) (factor * sin(angle));
		m[2] = 0.5f * (lx + rx);
		m[3] = -m[1];
		m[4] = m[0];
		float shift = (float) (factor * (WIDTH - LX - LY) / 2);
		m[5] = 0.5f * (ly + ry) + shift;

		CvMat M = cvMat(2, 3, CV_32F, m);

		IplImage* dst = cvCreateImage(cvSize(WIDTH, HEIGHT),8,3);

		cvGetQuadrangleSubPix(img, dst, &M);
		result.push_back( dst );
	}

	return result;
}
